import com.ridgesoft.robotics.Servo;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.sensors.CMUcam.*;

public class Camera implements Runnable{
	private static int CAM_SERVO_PORT = 3;
	private static int LEFT = 1;
	private static int RIGHT = 98;
	private static int FWD = 50;
	private static int F_LEFT = 25;
	private static int F_RIGHT = 75;

	//color tracking range values
	// 190 - 240 
	// 16  - 117
	// 16  - 23
	
	Servo camServo;
	CMUcam2 theCamera;
	CMUcamTrackingData trackingData;
	int midX;
	LCD d;
	
	private int servoPosition;
	
	public Camera()
	{
	}
	public Camera(LCD l)
	{
		this.d = l;
	}	
	
	public CMUcamTrackingData getTracking(){
		return theCamera.getTrackingData();
	}
	
	public void lookLeft(){
		camServo.setPosition(LEFT);
		servoPosition = 0;
	}
	
	public void lookFL(){
		camServo.setPosition(F_LEFT);
		servoPosition = 1;
	}

	public void lookFwd(){
		camServo.setPosition(FWD);
		servoPosition = 2;
	}
	
	public void lookFR(){
		camServo.setPosition(F_RIGHT);
		servoPosition = 3;
	}

	public void lookRight(){
		camServo.setPosition(RIGHT);
		servoPosition = 4;
	}
	
	public int getServoPos(){
		return servoPosition;
	}
	
	public void initTracking(){
		try{		
			theCamera.trackColor(120, 230, 0, 20, 0, 20);
		} catch(Exception e){
			d.printTop("wtf");
			e.printStackTrace();
		}
	}
	
	public void setCamLook(int w){
		switch(w){
		case 0:
			lookLeft();
			break;
		case 1:
			lookFL();
			break;
		case 2:
			lookFwd();
			break;
		case 3:
			lookFR();
			break;
		case 4:
			lookRight();
			break;
		}
	}
	
	public void scanLR(int speed){
		try{
			this.lookLeft();
			Thread.sleep(speed);
			this.lookFL();
			Thread.sleep(speed);
			this.lookFwd();
			Thread.sleep(speed);			
			this.lookFR();
			Thread.sleep(speed);
			this.lookRight();
			Thread.sleep(speed);
			
		} catch(Exception e) {
			e.printStackTrace();
		}
	}
	
	public void init()
	{
		try{
			//d.printBtm("init start");
			camServo = IntelliBrain.getServo(CAM_SERVO_PORT);
			camServo.setPosition(FWD);
			theCamera = new CMUcam2(IntelliBrain.getCom2(), 115200);
			//d.printTop("before open");
			theCamera.open();
			//d.printTop("after open");
			theCamera.setRGBMode(true);
			theCamera.setHighResolutionMode(true);
			theCamera.setAutoExposure(true);

		} catch(Exception e){
			d.printTop("error - init");
			e.printStackTrace();
		}
	}
	@Override
	public void run() {
		init();
		
		Thread.yield();
		
	}

}